#include "can_ros1_driver/can_receive.hpp"

// 0 初始化
canControl::canControl() : nh_("~")
{
    // 1 初始化ROS节点
    nh_.param<double>("wheel_base", wheel_base, 1.0);
    position_x_ = 0.0;
    position_y_ = 0.0;
    yaw = 0.0;

    /* 打开套接字 */
    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (0 > sockfd)
    {
        perror("socket error");
        exit(EXIT_FAILURE);
    }

    /* 指定can0设备 */
    ifr = {0};
    can_addr = {0};
    frame = {0};
    strcpy(ifr.ifr_name, "can0");
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    /* 将can0与套接字进行绑定 */
    if (0 > bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr)))
    {
        perror("bind error");
        close(sockfd);
        exit(EXIT_FAILURE);
    }

    // 创建用于反馈小车信息的发布者
    pub_ = nh_.advertise<can_ros1_driver::CarState>("/feedback_carState", 1000);
    // pub_Odom = nh_.advertise<nav_msgs::Odometry>("/panda_odom", 100);
    pub_radar_cluster = nh_.advertise<radar_perception_msgs::Cluster>("/radar_cluster", 10);
    pub_radar_object = nh_.advertise<radar_perception_msgs::Object>("/radar_obeject", 10);

    // can是否使用
    can_statu = false;
    // 毫米波是否使用
    mwlidar_statu = false;
    // 超声波是否使用
    ultrasonic_statu = false;
    // 雷达是否使用
    lidar_statu = false;
    camera_statu = false;
    // 用于判断接收数据是否超时
    ros::Time now_time = ros::Time::now();
    lidar_time = now_time.toSec();
    camera_time = now_time.toSec();
    mwlidar_time = now_time.toSec();
    ultrasonic_time = now_time.toSec();
    // sub_mwlidar = nh_.subscribe("/mw_lidar", 100, &canControl::mwlidarCallback, this);
    sub_ultrasonic = nh_.subscribe("/ultrasonic_l", 100, &canControl::ultrasonicCallback, this);
    sub_lidar = nh_.subscribe("/points_raw", 100, &canControl::lidarCallback, this);
    // sub_yolo = nh_.subscribe("/breakState", 1000, &canControl::yoloCallback, this);

    receiveCanData();
}

// void canControl::mwlidarCallback(const mw_lidar::mwlidar::ConstPtr &msg)
// {
//     ros::Time now_time = ros::Time::now();
//     mwlidar_time = now_time.toSec();
//     if (!mwlidar_statu)
//     {
//         if (msg != nullptr)
//         {
//             mwlidar_statu = true;
//         }
//         else
//             mwlidar_statu = false;
//     }
// }
void canControl::ultrasonicCallback(const sensor_msgs::Range::ConstPtr &msg)
{
    ros::Time now_time = ros::Time::now();
    ultrasonic_time = now_time.toSec();
    if (!ultrasonic_statu)
    {
        if (msg != nullptr)
        {
            ultrasonic_statu = true;
        }
        else
            ultrasonic_statu = false;
    }
}
void canControl::lidarCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
    ros::Time now_time = ros::Time::now();
    lidar_time = now_time.toSec();
    if (!lidar_statu)
    {
        if (msg != nullptr)
        {
            lidar_statu = true;
        }
        else
            lidar_statu = false;
    }
}
// void canControl::yoloCallback(const ultralytics_ros::breakState::ConstPtr &msg)
// {
//     ros::Time now_time = ros::Time::now();
//     camera_time = now_time.toSec();
//     if (!camera_statu)
//     {
//         if (msg != nullptr)
//         {
//             camera_statu = true;
//         }
//         else
//             camera_statu = false;
//     }
// }

// 2 接收CAN消息
void canControl::receiveCanData()
{
    while (ros::ok())
    {
        ros::spinOnce();
        /* 接收数据 */
        if (0 > read(sockfd, &frame, sizeof(struct can_frame)))
        {
            perror("read error");
            break;
        }

        /* 校验是否接收到错误帧 */
        if (frame.can_id & CAN_ERR_FLAG)
        {
            printf("Error frame!\n");
            break;
        }

        /* 校验帧格式 */
        // if (frame.can_id & CAN_EFF_FLAG) // 扩展帧
        //     printf("扩展帧 <0x%08x> \n", frame.can_id & CAN_EFF_MASK);
        // else // 标准帧
        //     printf("标准帧 <0x%03x> \n", frame.can_id & CAN_SFF_MASK);

        /* 校验帧类型：数据帧还是远程帧 */
        if (frame.can_id & CAN_RTR_FLAG)
        {
            printf("remote request\n");
            continue;
        }

        /* 打印数据长度 */
        // printf("[%d] ", frame.can_dlc);

        can_statu = true;

        /* 打印数据 */
        std::vector<uint8_t> buff;
        for (int i = 0; i < frame.can_dlc; i++)
        {
            // 这里的%02x表示以两位十六进制数的形式输出，并且若不足两位则在前面补0。
            // printf("%02x ", frame.data[i]);
            // 一位一位字节地添加到队列
            buff.push_back(static_cast<int8_t>(frame.data[i]));
        }
        // printf("\n");

        /* 解析数据 */
        if (frame.can_id == 0x116 && frame.can_dlc == 8)
        {
            // printf("扩展帧 <0x%08x> ", frame.can_id & CAN_EFF_MASK);
            // printf("%02x ", frame.can_dlc);
            // printf("\n---------------------\n");
            // printf("Receive状态值为:%02x \n", buff[0]);
            // printf("Receive打角值L为:%02x \n", buff[2]);
            // printf("Receive打角值H为:%02x \n", buff[3]);
            // printf("Receive速度值L为:%02x \n", buff[4]);
            // printf("Receive速度值H为:%02x \n", buff[5]);
            // printf("Receive电量0x为:%02x \n", buff[6]);

            std::vector<int16_t> feedback(10, 0);
            feedback = hexToShort(buff);

            // 获取完一帧的标志位,可以输出数据
            can_ros1_driver::CarState feedback_carState;

            // 1 获取车辆状态
            int gear = static_cast<int>(feedback[0]);
            feedback_carState.gear = gear;
            feedback_carState.driving_mode = static_cast<int>(feedback[4]);
            feedback_carState.brake = static_cast<int>(feedback[5]);
            feedback_carState.steering_machine_state = static_cast<int>(feedback[6]);
            feedback_carState.voltage_state = static_cast<int>(feedback[7]);
            feedback_carState.motor_state = static_cast<int>(feedback[8]);
            feedback_carState.throttle_opening = static_cast<int>(feedback[9]);
            double throttle = static_cast<double>(feedback[9]) / 100;
            feedback_carState.acceleration = throttle * 5 / 3;
            // 4 获取电池电量
            int power = static_cast<int>(feedback[3]);
            feedback_carState.battery_power = power;
            if (power < 20)
                ROS_WARN("Battery power (%d) is less than 10%!", power);

            // 3 获取车速
            // ± 5m/s 0.001(m/s)/bit  有符号
            double DM_Speed = static_cast<double>(feedback[2]) / 1000;
            if (gear == 2)
                DM_Speed = -DM_Speed;
            feedback_carState.speed = DM_Speed;
            
            // 2 获取舵机转向角度（打角值）
            // 524-1024-1524	0.1°/bit	0对应最左,1024对应中间角度
            double Steer_Angle = (static_cast<double>(feedback[1]) - 1024) / 10;
            // 打角值满足：tan(打角值) = 前后轮轴距 / 转弯半径
            // 角速度 = 线速度 / 转弯半径 = 线速度 * tan(打角值) / 前后轮轴距
            double Velocity_Angle = DM_Speed * tan(fabs(M_PI * Steer_Angle / 180)) / wheel_base;
            feedback_carState.steering = Steer_Angle;

            // std::cout << "Receive驾驶模式为: " << feedback_carState.driving_mode << std::endl;
            // std::cout << "Receive档位值为: " << gear << std::endl;
            // std::cout << "Receive刹车状态为: " << feedback_carState.brake << std::endl;
            // std::cout << "Receive转向机状态为: " << feedback_carState.steering_machine_state << std::endl;
            // std::cout << "Receive电压状态为: " << feedback_carState.voltage_state << std::endl;
            // std::cout << "Receive驱动电机状态为: " << feedback_carState.motor_state << std::endl;
            // std::cout << "Receive打角值为: " << Velocity_Angle << std::endl;
            // std::cout << "Receive速度值为: " << DM_Speed << std::endl;
            // std::cout << "Receive油门开度为: " << feedback_carState.throttle_opening << std::endl;
            // std::cout << "Receive加速度为: " << feedback_carState.acceleration << std::endl;
            // std::cout << "Receive电量为: " << feedback_carState.battery_power << std::endl;

            // 传感器状态
            feedback_carState.can_statu = can_statu;
            // 获取当前时间,超时故障
            ros::Time now_time = ros::Time::now();

            if (now_time.toSec() - mwlidar_time > 0.5)
                mwlidar_statu = false;
            feedback_carState.mwlidar_statu = mwlidar_statu;

            if (now_time.toSec() - ultrasonic_time > 1.5)
                ultrasonic_statu = false;
            feedback_carState.ultrasonic_statu = ultrasonic_statu;

            if (now_time.toSec() - lidar_time > 0.5)
                lidar_statu = false;
            feedback_carState.lidar_statu = lidar_statu;

            if (now_time.toSec() - camera_time > 0.5)
                camera_statu = false;
            feedback_carState.camera_statu = camera_statu;

            // 5 发布状态
            feedback_carState.header.stamp = ros::Time::now();
            pub_.publish(feedback_carState);

            // 6 发布里程计，左转是rZ正方向，而这个底盘最左是负，因此加-号
            // publishOdometry(DM_Speed, Velocity_Angle, -M_PI * Steer_Angle / 180);
        }
        else if(frame.can_id == 0x600) {
            radar_cluster.Cluster_1_General_Vec.clear();
            radar_cluster.Cluster_NofClustersNear = static_cast<int>(buff[0]);
            radar_cluster.Cluster_NofClustersFar = static_cast<int>(buff[1]);
            radar_cluster.Cluster_MeasCounter= static_cast<int>((buff[2] << 8) | buff[3]);
            radar_cluster.Cluster_InterfaceVersion = static_cast<int>(buff[4] >> 4);
        }
        else if(frame.can_id == 0x701) {
            radar_perception_msgs::ClusterVec clusterVec;
            clusterVec.Cluster_ID = static_cast<int>(buff[0]);
            clusterVec.Cluster_DistLong = ((buff[1] << 5) | (buff[2] >> 3)) * 0.2 - 500;
            clusterVec.Cluster_DistLat = (((buff[2] & 0x03) << 8) | (buff[3])) * 0.2 - 102.3;
            clusterVec.Cluster_VrelLong = ((buff[4] << 2) | buff[5] >> 6) * 0.25 - 128.00;
            clusterVec.Cluster_VrelLat = (((buff[5] & 0x3f) << 3) | (buff[6] >> 5)) * 0.25 - 64.00;
            clusterVec.Cluster_DynProp = buff[6] & 0x07;
            clusterVec.Cluster_RCS = buff[7] * 0.5 - 64.0; 
            radar_cluster.Cluster_1_General_Vec.push_back(clusterVec);
            if(radar_cluster.Cluster_1_General_Vec.size() == (radar_cluster.Cluster_NofClustersNear + radar_cluster.Cluster_NofClustersFar))
                pub_radar_cluster.publish(radar_cluster);
        }
        else if(frame.can_id == 0x60A) {
            radar_object.Object_1_General_Vec.clear();
            radar_object.Object_NofObjects = buff[0];
            radar_object.Object_MeasCounter = (buff[1] << 8) | buff[2];
            radar_object.Object_InterfaceVersion = (buff[3] >> 4);
        } 
        else if(frame.can_id == 0x60B) {
            radar_perception_msgs::ObjectVec objectVec;
            objectVec.Object_ID = buff[0];
            objectVec.Object_DistLong = ((buff[1] << 5) | (buff[2] >> 3)) * 0.2 - 500;
            objectVec.Object_DistLat = (((buff[2] & 0x07) << 8) | buff[3]) * 0.2 - 204.6;
            objectVec.Object_VrelLong = ((buff[4] << 2) | (buff[5] >> 6)) * 0.25 - 128.00;
            objectVec.Object_VrelLat = (((buff[5] & 0x3f) << 3) | (buff[6] >> 5)) * 0.25 -64.00;
            objectVec.Object_DynProp = buff[6] & 0x07;
            objectVec.Object_RCS = buff[7] * 0.5 - 64.0;
            radar_object.Object_1_General_Vec.push_back(objectVec);
            if(radar_object.Object_1_General_Vec.size() == radar_object.Object_NofObjects)
                pub_radar_object.publish(radar_object);
        }
    }
}

// 2.1 解析数据
std::vector<int16_t> canControl::hexToShort(const std::vector<uint8_t> &hex_data)
{
    std::vector<int16_t> short_data(10, 0);
    // 1 获取档位
    uint8_t byte = hex_data[0], mask = 0b00001100;
    // 使用位与操作符&来获取第3-4位的值,将结果向右位移2位
    uint8_t gear = (byte & mask) >> 2;
    short_data[0] = static_cast<int16_t>(gear);

    // 2 获取舵机转向角度
    // 将两个连续的字节（低位hex_data[i] 和 高位hex_data[i+1]）组合为一个 int16_t 类型的数值：千万注意高低顺序，仔细看通讯协议
    // 高位hex_data[i+1]需要先强制转换为一个有符号的short类型的数据以后再移位
    short Steer_Angle_H = static_cast<int16_t>(hex_data[3]);
    // 左移运算符 << 将 high 的二进制表示向左移动 8 位。这样做是因为 int16_t 类型占用 2 个字节，而我们希望将 high 的数据放置在最高的 8 位上。
    // |: 按位或运算符 | 将经过左移的 high 数据和 hex_data[i] 数据进行按位或操作，将它们组合为一个 16 位的数值
    short_data[1] = static_cast<int16_t>((Steer_Angle_H << 8) | hex_data[2]);

    // 3 获取车速
    short DM_Speed_H = static_cast<int16_t>(hex_data[5]);
    short_data[2] = static_cast<int16_t>((DM_Speed_H << 8) | hex_data[4]);

    // 4 获取电池电量
    short_data[3] = static_cast<int16_t>(hex_data[6]);

    // 5 获取其他状态
    mask = 0b00000010;
    // 使用位与操作符&来获取第2位的值,将结果向右位移1位
    uint8_t driving_mode = (byte & mask) >> 1;
    short_data[4] = static_cast<int16_t>(driving_mode);

    mask = 0b00010000;
    // 使用位与操作符&来获取第5位的值,将结果向右位移4位
    uint8_t brake = (byte & mask) >> 4;
    short_data[5] = static_cast<int16_t>(brake);

    mask = 0b00100000;
    // 使用位与操作符&来获取第6位的值,将结果向右位移5位
    uint8_t steering_machine_state = (byte & mask) >> 5;
    short_data[6] = static_cast<int16_t>(steering_machine_state);

    mask = 0b01000000;
    // 使用位与操作符&来获取第7位的值,将结果向右位移6位
    uint8_t voltage_state = (byte & mask) >> 6;
    short_data[7] = static_cast<int16_t>(voltage_state);

    mask = 0b10000000;
    // 使用位与操作符&来获取第8位的值,将结果向右位移7位
    uint8_t motor_state = (byte & mask) >> 7;
    short_data[8] = static_cast<int16_t>(motor_state);

    // 油门开度
    short_data[9] = static_cast<int16_t>(hex_data[1]);

    return short_data;
}

// 2.2 推算里程计并发布
void canControl::publishOdometry(double linear_velocity, double angular_velocity, double Steer_Angle)
{
    // 计算与上一时刻的时间差
    static double last_stamp = ros::Time::now().toSec();
    double dt = ros::Time::now().toSec() - last_stamp;
    last_stamp = ros::Time::now().toSec();

    // 1 推算里程
    double wz = angular_velocity, vx = 0.0, vy = 0.0;
    // 计算偏航角（注意是相对于初始0，要绝对的需要IMU）
    yaw += angular_velocity * dt;
    // 根据线速度以及航向角计算X，Y方向上的位移
    vx = linear_velocity * std::cos(Steer_Angle);
    vy = linear_velocity * (linear_velocity < 0.0 ? sin(-Steer_Angle) : sin(Steer_Angle));
    // 计算相对与上一时刻的位，根据偏航变换到初始坐标系
    position_x_ += cos(yaw) * vx * dt - sin(yaw) * vy * dt;
    position_y_ += sin(yaw) * vx * dt + cos(yaw) * vy * dt;

    // 2 发布tf
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw);
    // std::cout<< "odom_quat:" << odom_quat<<std::endl;
    geometry_msgs::TransformStamped tf_msg;
    tf_msg.header.stamp = ros::Time::now();
    tf_msg.header.frame_id = "odom";
    tf_msg.child_frame_id = "base_link";
    tf_msg.transform.translation.x = position_x_;
    tf_msg.transform.translation.y = position_y_;
    tf_msg.transform.translation.z = 0.0;
    tf_msg.transform.rotation = odom_quat;
    tf_broadcaster_.sendTransform(tf_msg);

    // 3 发布odom
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_link";

    odom_msg.pose.pose.position.x = position_x_;
    odom_msg.pose.pose.position.y = position_y_;
    odom_msg.pose.pose.position.z = 0.0;
    odom_msg.pose.pose.orientation = odom_quat;

    odom_msg.twist.twist.linear.x = vx;
    odom_msg.twist.twist.linear.y = vy;
    odom_msg.twist.twist.angular.z = wz;

    odom_msg.pose.covariance[0] = 0.1;
    odom_msg.pose.covariance[7] = 0.1;
    odom_msg.pose.covariance[14] = 0.1;
    odom_msg.pose.covariance[21] = 1.0;
    odom_msg.pose.covariance[28] = 1.0;
    odom_msg.pose.covariance[35] = 1.0;

    pub_Odom.publish(odom_msg);
}

// 3 析构函数
canControl::~canControl()
{
    /* 关闭套接字 */
    close(sockfd);
    exit(EXIT_SUCCESS);
}
